using System;
using System.Drawing;
using System.IO;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace MPSizectorDotNet
{
	public class MP3DFrameManaged
	{
		public uint SensorWidth;

		public uint SensorHeight;

		public uint SensorResolution;

		public float XMax;

		public float YMax;

		public float ZMax;

		public MP3DDataStruct[] Data;

		private Color[] m_ColorArr = new Color[10001];

		private float m_LastColorDepthMax = 0f;

		private float m_LastColorDepthMin = 0f;

		private static Color[] DepthColorStops = new Color[5]
		{
			Color.Purple,
			Color.Blue,
			Color.Green,
			Color.Yellow,
			Color.Red
		};

		private static Color TooHighColor = Color.FromArgb(230, 230, 230);

		private static Color TooLowColor = Color.FromArgb(20, 20, 20);

		public MP3DFrameManaged(MP3DFrameManaged SourceToCopy)
		{
			this.SensorWidth = SourceToCopy.SensorWidth;
			this.SensorHeight = SourceToCopy.SensorHeight;
			this.SensorResolution = SourceToCopy.SensorResolution;
			this.XMax = SourceToCopy.XMax;
			this.YMax = SourceToCopy.YMax;
			this.ZMax = SourceToCopy.ZMax;
			int length = SourceToCopy.Data.GetLength(0);
			this.Data = new MP3DDataStruct[length];
			for (int i = 0; i < length; i++)
			{
				this.Data[i] = SourceToCopy.Data[i];
			}
		}

		public MP3DFrameManaged(uint ImgWidth, uint ImgHeight)
		{
			this.SensorWidth = ImgWidth;
			this.SensorHeight = ImgHeight;
			this.SensorResolution = ImgWidth * ImgHeight;
			this.XMax = 30f;
			this.YMax = 22.5f;
			this.ZMax = 15f;
			this.Data = new MP3DDataStruct[ImgWidth * ImgHeight];
		}

		public MP3DFrameManaged(string FileName)
		{
			this.SensorWidth = 752u;
			this.SensorHeight = 480u;
			this.SensorResolution = this.SensorWidth * this.SensorHeight;
			this.XMax = 30f;
			this.YMax = 22.5f;
			this.ZMax = 15f;
			this.Load(FileName);
		}

		public bool Save(string FileName)
		{
			try
			{
				FileStream fileStream = new FileStream(FileName, FileMode.Create);
				BinaryWriter binaryWriter = new BinaryWriter(fileStream);
				float value = 2.1f;
				binaryWriter.Write(this.SensorWidth);
				binaryWriter.Write(this.SensorHeight);
				binaryWriter.Write(value);
				binaryWriter.Write(this.XMax);
				binaryWriter.Write(this.YMax);
				binaryWriter.Write(this.ZMax);
				uint num = this.SensorWidth * this.SensorHeight;
				for (int i = 0; i < num; i++)
				{
					binaryWriter.Write(this.Data[i].Gray);
					binaryWriter.Write(this.Data[i].Mask);
					binaryWriter.Write(this.Data[i].X);
					binaryWriter.Write(this.Data[i].Y);
					binaryWriter.Write(this.Data[i].Z);
				}
				binaryWriter.Close();
				fileStream.Close();
				return true;
			}
			catch
			{
				return false;
			}
		}

		public bool Load(string FileName)
		{
			FileStream fileStream = new FileStream(FileName, FileMode.Open);
			BinaryReader binaryReader = new BinaryReader(fileStream);
			long length = fileStream.Length;
			this.SensorWidth = (uint)binaryReader.ReadInt32();
			this.SensorHeight = (uint)binaryReader.ReadInt32();
			this.SensorResolution = this.SensorWidth * this.SensorHeight;
			if (length < this.SensorResolution * 14 + 8)
			{
				MessageBox.Show("Wrong file length: Too short file");
				binaryReader.Close();
				fileStream.Close();
				throw new FileLoadException();
			}
			if (length == this.SensorResolution * 14 + 8)
			{
				this.XMax = 30f;
				this.YMax = 22.5f;
				this.ZMax = 15f;
				goto IL_0126;
			}
			float num = binaryReader.ReadSingle();
			if (num == 2.1f)
			{
				this.XMax = binaryReader.ReadSingle();
				this.YMax = binaryReader.ReadSingle();
				this.ZMax = binaryReader.ReadSingle();
				goto IL_0126;
			}
			MessageBox.Show("Unsupported version of file: v" + num.ToString());
			binaryReader.Close();
			fileStream.Close();
			throw new FileLoadException();
			IL_0126:
			this.Data = new MP3DDataStruct[this.SensorResolution];
			for (int i = 0; i < this.SensorResolution; i++)
			{
				this.Data[i].Gray = binaryReader.ReadByte();
				this.Data[i].Mask = binaryReader.ReadByte();
				this.Data[i].X = binaryReader.ReadSingle();
				this.Data[i].Y = binaryReader.ReadSingle();
				this.Data[i].Z = binaryReader.ReadSingle();
			}
			binaryReader.Close();
			fileStream.Close();
			return true;
		}

		public bool Save3DPointCloud(string FileName)
		{
			uint num = this.SensorWidth * this.SensorHeight;
			try
			{
				StreamWriter streamWriter = new StreamWriter(FileName, false, Encoding.Default);
				for (int i = 0; i < num; i++)
				{
					if (this.Data[i].Mask == 0)
					{
						streamWriter.WriteLine(this.Data[i].X.ToString() + "\t" + this.Data[i].Y.ToString() + "\t" + this.Data[i].Z.ToString() + "\r\n");
					}
				}
				streamWriter.Close();
			}
			catch
			{
				return false;
			}
			return true;
		}

		private bool MendPointSafe(MP3DDataStruct[] Data, uint idx)
		{
			if (Data[idx].Mask != 0)
			{
				uint num = idx / this.SensorWidth;
				uint num2 = idx % this.SensorWidth;
				if (num2 < 2 || num < 2 || num2 > this.SensorWidth - 3 || num > this.SensorHeight - 3)
				{
					return false;
				}
				uint num3 = idx - 1;
				uint num4 = idx + 1;
				bool flag = Data[num3].Mask == 0;
				bool flag2 = Data[num4].Mask == 0;
				if (flag && flag2)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num3].X + Data[num4].X) / 2f;
					Data[idx].Y = (Data[num3].Y + Data[num4].Y) / 2f;
					Data[idx].Z = (Data[num3].Z + Data[num4].Z) / 2f;
					return true;
				}
				uint num5 = idx - this.SensorWidth;
				uint num6 = idx + this.SensorWidth;
				bool flag3 = Data[num5].Mask == 0;
				bool flag4 = Data[num6].Mask == 0;
				if (flag3 && flag4)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num5].X + Data[num6].X) / 2f;
					Data[idx].Y = (Data[num5].Y + Data[num6].Y) / 2f;
					Data[idx].Z = (Data[num5].Z + Data[num6].Z) / 2f;
					return true;
				}
				uint num7 = num5 - 1;
				uint num8 = num6 + 1;
				bool flag5 = Data[num7].Mask == 0;
				bool flag6 = Data[num8].Mask == 0;
				if (flag5 && flag6)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num7].X + Data[num8].X) / 2f;
					Data[idx].Y = (Data[num7].Y + Data[num8].Y) / 2f;
					Data[idx].Z = (Data[num7].Z + Data[num8].Z) / 2f;
					return true;
				}
				uint num9 = num5 + 1;
				uint num10 = num6 - 1;
				bool flag7 = Data[num9].Mask == 0;
				bool flag8 = Data[num10].Mask == 0;
				if (flag7 && flag8)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num9].X + Data[num10].X) / 2f;
					Data[idx].Y = (Data[num9].Y + Data[num10].Y) / 2f;
					Data[idx].Z = (Data[num9].Z + Data[num10].Z) / 2f;
					return true;
				}
				uint num11 = idx - 2;
				uint num12 = idx + 2;
				bool flag9 = Data[num11].Mask == 0;
				bool flag10 = Data[num12].Mask == 0;
				if (flag9 && flag10)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num11].X + Data[num12].X) / 2f;
					Data[idx].Y = (Data[num11].Y + Data[num12].Y) / 2f;
					Data[idx].Z = (Data[num11].Z + Data[num12].Z) / 2f;
					return true;
				}
				uint num13 = idx - this.SensorWidth * 2;
				uint num14 = idx + this.SensorWidth * 2;
				bool flag11 = Data[num13].Mask == 0;
				bool flag12 = Data[num14].Mask == 0;
				if (flag3 && flag4)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num13].X + Data[num14].X) / 2f;
					Data[idx].Y = (Data[num13].Y + Data[num14].Y) / 2f;
					Data[idx].Z = (Data[num13].Z + Data[num14].Z) / 2f;
					return true;
				}
				uint num15 = num13 - 2;
				uint num16 = num14 + 2;
				bool flag13 = Data[num15].Mask == 0;
				bool flag14 = Data[num16].Mask == 0;
				if (flag3 && flag4)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num15].X + Data[num16].X) / 2f;
					Data[idx].Y = (Data[num15].Y + Data[num16].Y) / 2f;
					Data[idx].Z = (Data[num15].Z + Data[num16].Z) / 2f;
					return true;
				}
				uint num17 = num13 + 2;
				uint num18 = num14 - 2;
				bool flag15 = Data[num17].Mask == 0;
				bool flag16 = Data[num18].Mask == 0;
				if (flag3 && flag4)
				{
					Data[idx].Mask = 0;
					Data[idx].X = (Data[num17].X + Data[num18].X) / 2f;
					Data[idx].Y = (Data[num17].Y + Data[num18].Y) / 2f;
					Data[idx].Z = (Data[num17].Z + Data[num18].Z) / 2f;
					return true;
				}
			}
			return false;
		}

		public void PostProcessRemoveBurrs()
		{
			MP3DDataStruct[] NewData = new MP3DDataStruct[this.SensorWidth * this.SensorHeight];
			DateTime now = DateTime.Now;
			float num = this.XMax / 0.75f;
			float num2 = num / (float)(double)this.SensorWidth;
			float SlopTh = 0f - num2 * 2f;
			if (this.SensorWidth > 728)
			{
				SlopTh *= 2f;
			}
			Parallel.For(0L, this.SensorHeight, delegate(long y)
			{
				for (uint num3 = 0u; num3 < this.SensorWidth; num3++)
				{
					uint num4 = (uint)(y * this.SensorWidth + num3);
					NewData[num4] = this.Data[num4];
					if (NewData[num4].X < 0f - this.XMax || NewData[num4].X > this.XMax || NewData[num4].Y < 0f - this.YMax || NewData[num4].Y > this.YMax || NewData[num4].Z < 0f - this.ZMax || NewData[num4].Z > this.ZMax)
					{
						NewData[num4].Mask = 8;
					}
					else if (this.Data[num4].Mask == 0 && num3 > 1 && num3 < this.SensorWidth - 1 && y > 1 && y < this.SensorHeight - 1)
					{
						uint num5 = num4 - 1;
						if (this.Data[num5].Mask == 0)
						{
							float num6 = this.Data[num4].X - this.Data[num5].X;
							float num7 = this.Data[num4].Z - this.Data[num5].Z;
							if (num6 <= 0f)
							{
								NewData[num4].Mask = 8;
							}
							else if (num7 < SlopTh)
							{
								NewData[num4].Mask = 8;
							}
						}
						uint num8 = num4 + 1;
						if (this.Data[num8].Mask == 0)
						{
							float num9 = this.Data[num8].X - this.Data[num4].X;
							float num10 = this.Data[num8].Z - this.Data[num4].Z;
							if (num9 <= 0f)
							{
								NewData[num4].Mask = 8;
							}
							else if (num10 < SlopTh)
							{
								NewData[num4].Mask = 8;
							}
						}
					}
				}
			});
			this.Data = NewData;
			GC.Collect();
			DateTime now2 = DateTime.Now;
			TimeSpan timeSpan = now2 - now;
			now = now2;
		}

		public void PostProcessFiltrate()
		{
			MP3DDataStruct[] NewData = new MP3DDataStruct[this.SensorWidth * this.SensorHeight];
			DateTime now = DateTime.Now;
			Parallel.For(0L, this.SensorHeight, delegate(long y)
			{
				float num = this.XMax / 0.75f / (float)(double)this.SensorWidth * 5f;
				if (this.SensorWidth > 728)
				{
					num *= 2f;
				}
				for (uint num2 = 0u; num2 < this.SensorWidth; num2++)
				{
					uint num3 = (uint)(y * this.SensorWidth + num2);
					NewData[num3] = this.Data[num3];
					if (num2 > 1 && num2 < this.SensorWidth - 1 && y > 1 && y < this.SensorHeight - 1)
					{
						uint num4 = num3 - 1;
						uint num5 = num3 + 1;
						uint num6 = num3 - this.SensorWidth;
						uint num7 = num6 - 1;
						uint num8 = num6 + 1;
						uint num9 = num3 + this.SensorWidth;
						uint num10 = num9 - 1;
						uint num11 = num9 + 1;
						if (this.Data[num3].Mask == 0 && this.Data[num4].Mask == 0 && this.Data[num5].Mask == 0 && this.Data[num6].Mask == 0 && this.Data[num7].Mask == 0 && this.Data[num8].Mask == 0 && this.Data[num9].Mask == 0 && this.Data[num10].Mask == 0 && this.Data[num11].Mask == 0)
						{
							float num12 = this.Data[num3].Z - num;
							float num13 = this.Data[num3].Z + num;
							if (this.Data[num4].Z < num13 && this.Data[num4].Z > num12 && this.Data[num5].Z < num13 && this.Data[num5].Z > num12 && this.Data[num6].Z < num13 && this.Data[num6].Z > num12 && this.Data[num7].Z < num13 && this.Data[num7].Z > num12 && this.Data[num8].Z < num13 && this.Data[num8].Z > num12 && this.Data[num9].Z < num13 && this.Data[num9].Z > num12 && this.Data[num10].Z < num13 && this.Data[num10].Z > num12 && this.Data[num11].Z < num13 && this.Data[num11].Z > num12)
							{
								NewData[num3].Z = (this.Data[num3].Z + this.Data[num4].Z + this.Data[num5].Z + this.Data[num6].Z + this.Data[num7].Z + this.Data[num8].Z + this.Data[num9].Z + this.Data[num10].Z + this.Data[num11].Z) / 9f;
							}
						}
					}
				}
			});
			this.Data = NewData;
			GC.Collect();
			DateTime now2 = DateTime.Now;
			TimeSpan timeSpan = now2 - now;
			now = now2;
		}

		public void PostProcessMend()
		{
			uint[] array = new uint[this.SensorHeight * this.SensorWidth];
			uint[] array2 = new uint[this.SensorHeight * this.SensorWidth];
			uint num = 0u;
			uint num2 = 1u;
			DateTime now = DateTime.Now;
			Parallel.For(2L, this.SensorHeight - 2, delegate(long y)
			{
				for (uint num3 = 2u; num3 < this.SensorWidth - 2; num3++)
				{
					this.MendPointSafe(this.Data, (uint)(y * this.SensorWidth + num3));
				}
			});
			DateTime now2 = DateTime.Now;
			TimeSpan timeSpan = now2 - now;
			now = now2;
		}

		public unsafe Bitmap GetGrayBitmap()
		{
			uint num = this.SensorWidth * this.SensorHeight;
			int num2 = Marshal.SizeOf(typeof(MP3DDataStruct));
			if (this.SensorWidth == 0 || this.SensorHeight == 0)
			{
				throw new ArgumentOutOfRangeException();
			}
			if (this.Data.Length != num)
			{
				throw new ArgumentOutOfRangeException();
			}
			byte[] array = new byte[this.SensorWidth * this.SensorHeight];
			byte[] array2 = array;
			fixed (byte* ptr = array2)
			{
				MP3DDataStruct[] data = this.Data;
				fixed (MP3DDataStruct* ptr3 = data)
				{
					byte* ptr2 = ptr;
					byte* ptr4 = (byte*)ptr3;
					ptr4 = ptr4;
					for (int i = 0; i < num; i++)
					{
						byte* intPtr = ptr2;
						ptr2 = intPtr + 1;
						*intPtr = *ptr4;
						ptr4 += num2;
					}
				}
			}
			return Utils.ConvertGrayByteArrayToGrayBitmap(array, this.SensorWidth, this.SensorHeight);
		}

		public unsafe Bitmap Get3DMaskBitmap()
		{
			uint num = this.SensorWidth * this.SensorHeight;
			int num2 = Marshal.SizeOf(typeof(MP3DDataStruct));
			if (this.SensorWidth == 0 || this.SensorHeight == 0)
			{
				throw new ArgumentOutOfRangeException();
			}
			if (this.Data.Length != num)
			{
				throw new ArgumentOutOfRangeException();
			}
			byte[] array = new byte[this.SensorWidth * this.SensorHeight];
			byte[] array2 = new byte[this.SensorWidth * this.SensorHeight];
			byte[] array3 = new byte[this.SensorWidth * this.SensorHeight];
			byte[] array4 = array;
			fixed (byte* ptr = array4)
			{
				array4 = array2;
				fixed (byte* ptr3 = array4)
				{
					array4 = array3;
					fixed (byte* ptr5 = array4)
					{
						MP3DDataStruct[] data = this.Data;
						fixed (MP3DDataStruct* ptr7 = data)
						{
							byte* ptr2 = ptr;
							byte* ptr4 = ptr3;
							byte* ptr6 = ptr5;
							byte* ptr8 = (byte*)ptr7;
							ptr8++;
							for (int i = 0; i < num; i++)
							{
								byte b = *ptr8;
								bool flag = (b & 1) == 1;
								bool flag2 = (b & 2) == 2;
								bool flag3 = (b & 4) == 4;
								bool flag4 = (b & 8) == 8;
								if (b == 0)
								{
									*ptr2 = 0;
									*ptr4 = 255;
									*ptr6 = 0;
								}
								else if (flag2)
								{
									*ptr2 = 255;
									*ptr4 = 255;
									*ptr6 = 255;
								}
								else if (flag3)
								{
									*ptr2 = 0;
									*ptr4 = 0;
									*ptr6 = 0;
								}
								else if (flag)
								{
									*ptr2 = 0;
									*ptr4 = 0;
									*ptr6 = 255;
								}
								else if (flag4)
								{
									*ptr2 = 255;
									*ptr4 = 0;
									*ptr6 = 0;
								}
								ptr2++;
								ptr4++;
								ptr6++;
								ptr8 += num2;
							}
						}
					}
				}
			}
			return Utils.ConvertRGBByteArrayToRGBBitmap(array, array2, array3, this.SensorWidth, this.SensorHeight);
		}

		private void PrepareColorArr(float Max, float Min)
		{
			if (Max != this.m_LastColorDepthMax || Min != this.m_LastColorDepthMin)
			{
				for (int i = 0; i <= 10000; i++)
				{
					this.m_ColorArr[i] = MP3DFrameManaged.GetDepthColor((float)i / 10000f);
				}
				this.m_LastColorDepthMax = Max;
				this.m_LastColorDepthMin = Min;
			}
		}

		public Bitmap GetColorDepthBitmap(Convert3DTargetType Target, float Max, float Min)
		{
			uint num = this.SensorWidth * this.SensorHeight;
			int num2 = Marshal.SizeOf(typeof(MP3DDataStruct));
			if (this.SensorWidth == 0 || this.SensorHeight == 0)
			{
				throw new ArgumentOutOfRangeException();
			}
			if (this.Data.Length != num)
			{
				throw new ArgumentOutOfRangeException();
			}
			byte[] Red = new byte[this.SensorWidth * this.SensorHeight];
			byte[] Green = new byte[this.SensorWidth * this.SensorHeight];
			byte[] Blue = new byte[this.SensorWidth * this.SensorHeight];
			this.PrepareColorArr(Max, Min);
			Parallel.For(0L, num, delegate(long Index)
			{
				byte mask = this.Data[Index].Mask;
				float num3 = (Target != Convert3DTargetType.Z) ? ((Target != Convert3DTargetType.Y) ? this.Data[Index].X : this.Data[Index].Y) : this.Data[Index].Z;
				float num4 = (num3 - Min) / (Max - Min);
				int num5 = (int)(num4 * 10000f);
				if (num5 > 10000)
				{
					num5 = 10000;
				}
				else if (num5 < 0)
				{
					num5 = 0;
				}
				if (mask != 0 || float.IsNaN(num3))
				{
					Red[Index] = 0;
					Green[Index] = 0;
					Blue[Index] = 0;
				}
				else if (float.IsPositiveInfinity(num3))
				{
					Red[Index] = 220;
					Green[Index] = 220;
					Blue[Index] = 220;
				}
				else if (float.IsNegativeInfinity(num3))
				{
					Red[Index] = 20;
					Green[Index] = 20;
					Blue[Index] = 20;
				}
				else
				{
					Color color = this.m_ColorArr[num5];
					Red[Index] = color.R;
					Green[Index] = color.G;
					Blue[Index] = color.B;
				}
			});
			return Utils.ConvertRGBByteArrayToRGBBitmap(Red, Green, Blue, this.SensorWidth, this.SensorHeight);
		}

		public static Color GetDepthColor(float Ratio)
		{
			if (Ratio >= 1f)
			{
				return MP3DFrameManaged.TooHighColor;
			}
			if (Ratio <= 0f)
			{
				return MP3DFrameManaged.TooLowColor;
			}
			int num = (int)(Ratio * 4f);
			int num2 = num + 1;
			float num3 = (float)num * 0.25f;
			float num4 = (float)num2 * 0.25f;
			Color color = MP3DFrameManaged.DepthColorStops[num];
			Color color2 = MP3DFrameManaged.DepthColorStops[num2];
			Ratio = (float)Math.Round((double)((Ratio - num3) / (num4 - num3)), 2);
			byte red = (byte)((float)(color2.R - color.R) * Ratio + (float)(int)color.R);
			byte green = (byte)((float)(color2.G - color.G) * Ratio + (float)(int)color.G);
			byte blue = (byte)((float)(color2.B - color.B) * Ratio + (float)(int)color.B);
			return Color.FromArgb(red, green, blue);
		}
	}
}
